#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joy.hpp"
#include "xbox.hpp"
/**
 * @brief Xbox joystick input to Ros message Joy
 * 
 */
class joy_node : public rclcpp::Node
{
	private:
	rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_pub;
	std::shared_ptr<xbox_input> xbox;
	std::thread recv_thread;
	public:
	void recv_thread_func()
	{
		js_event event;
		int ret;
		std::vector<float> axis(8);
		std::vector<int> button(8);
		axis[4] = -1;
		axis[5] = -1;

		while (rclcpp::ok())
		{
			sensor_msgs::msg::Joy joy;
			ret = this->xbox->read_event(event);
			// std::cout << " event: " << (unsigned int)event.type << " axis: " << (unsigned int)event.number << " number: " << event.value << std::endl;
			if (ret > 0)
			{
				if (event.type == 2 && event.number < 8)
					axis[event.number] = event.value / 32767.0;
				if (event.type == 1 && event.number < 8)
					button[event.number] = event.value;
			}
			else
			{
				std::cout << "failed to read device file, try reopen" << std::endl;
				if (this->xbox->dev_open() < 0)
					std::this_thread::sleep_for(std::chrono::milliseconds(1000));
				else
					std::cout << "successfully reopen device file" << std::endl;
				continue;
			}
			joy.header.frame_id = "joystick";
			joy.header.stamp = rclcpp::Clock().now();
			joy.axes = axis;
			joy.buttons = button;
			this->joy_pub->publish(joy);
		}

	}
	joy_node(std::string name) : Node(name)
	{
		this->joy_pub = this->create_publisher<sensor_msgs::msg::Joy>("xbox_joy", 1);
		this->xbox = std::make_shared<xbox_input>();
		this->recv_thread = std::thread(std::bind(&joy_node::recv_thread_func, this));
		this->recv_thread.detach();
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<joy_node>("joy_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}




